#include <ros/ros.h>
#include "kinematic.h"
#include <ubt_core_msgs/JointCommand.h>
#include "webots_api/SceneSelection.h"
#include <sensor_msgs/JointState.h>
#include "walk.h"

void data_to_leg(sensor_msgs::JointState &joint_state, float *joints)
{
    joint_state.position[0] = joints[0];
    joint_state.position[1] = joints[1];
    joint_state.position[2] = joints[2];
    joint_state.position[3] = joints[3];
    joint_state.position[4] = joints[4];
    joint_state.position[5] = joints[5];
    joint_state.position[6] = joints[6];
    joint_state.position[7] = joints[7];
    joint_state.position[8] = joints[8];
    joint_state.position[9] = joints[9];
    joint_state.position[10] = joints[10];
    joint_state.position[11] = joints[11];
}

int main(int argc, char **argv) {
    Matrix_Init();
    ros::init(argc, argv, "CarryBox_node");
    ros::NodeHandle nh;

    ros::ServiceClient client = nh.serviceClient<webots_api::SceneSelection>("/walker/sence");

    webots_api::SceneSelection scene_selection;
    scene_selection.request.scene_name = "CarryBox";
    scene_selection.request.nav = false;
    scene_selection.request.vision = false;

    std::cout << "Wait for SceneSelect Service" << std::endl;
    while(!client.call(scene_selection))
    {
        if(!ros::ok())
        {
            return 0;
        }
    }
    ROS_INFO("Scene select success!");

    ros::Publisher leg_pub = nh.advertise<sensor_msgs::JointState>("/Leg/DesiredJoint", 10);
    sensor_msgs::JointState leg_joints;
    leg_joints.position.resize(12);
    
    Walk* leg_walk = new Walk();
    float joints[12] = {0};

    ros::Publisher left_arm_pub = nh.advertise<ubt_core_msgs::JointCommand>("/walker/leftLimb/controller", 10);
    ubt_core_msgs::JointCommand left_arm_data;
    left_arm_data.command.resize(7);
    left_arm_data.mode = 5;

    ros::Publisher left_finger_pub = nh.advertise<ubt_core_msgs::JointCommand>("/walker/leftHand/controller", 10);
    ubt_core_msgs::JointCommand left_finger_data;
    left_finger_data.command.resize(10);
    left_finger_data.mode = 5;

    ros::Publisher right_arm_pub = nh.advertise<ubt_core_msgs::JointCommand>("/walker/rightLimb/controller", 10);
    ubt_core_msgs::JointCommand right_arm_data;
    right_arm_data.command.resize(7);
    right_arm_data.mode = 5;

    ros::Publisher right_finger_pub = nh.advertise<ubt_core_msgs::JointCommand>("/walker/rightHand/controller", 10);
    ubt_core_msgs::JointCommand right_finger_data;
    right_finger_data.command.resize(10);
    right_finger_data.mode = 5;

    Eigen::VectorXd left_joints(7);
    left_joints << 0,0,0,0,0,0,0;
    Eigen::VectorXd right_joints(7);
    right_joints << 0,0,0,0,0,0,0;

    Eigen::Vector3d position, orientation;

    ros::Rate loop_rate(1000);
    double time_count = 0.0;
    while(ros::ok())
    {
        time_count += 0.002;
        if(time_count < 0.5)
        {
            if(leg_walk->get_status() == "standing")
            {
                leg_walk->Init(joints, 50, true);
                data_to_leg(leg_joints, joints);
                leg_pub.publish(leg_joints);
            }
            position << 400, 0, 400-200*time_count/0.5;
            if(leftArmInverse(position, orientation, left_joints, false) == true)
            {
                left_arm_data.command[0] = left_joints(0);
                left_arm_data.command[1] = left_joints(1);
                left_arm_data.command[2] = left_joints(2);
                left_arm_data.command[3] = left_joints(3);
                left_arm_data.command[4] = left_joints(4);
                left_arm_data.command[5] = left_joints(5);
                left_arm_data.command[6] = left_joints(6);
                left_arm_pub.publish(left_arm_data);
            }

            position << -400, 0, 400-200*time_count/0.5;
            if(rightArmInverse(position, orientation, right_joints, false) == true)
            {
                right_arm_data.command[0] = right_joints(0);
                right_arm_data.command[1] = right_joints(1);
                right_arm_data.command[2] = right_joints(2);
                right_arm_data.command[3] = right_joints(3);
                right_arm_data.command[4] = right_joints(4);
                right_arm_data.command[5] = right_joints(5);
                right_arm_data.command[6] = right_joints(6);
                right_arm_pub.publish(right_arm_data);
            }
        }
        else if(time_count < 1)
        {
            if(leg_walk->get_status() == "standing")
            {
                leg_walk->Init(joints, 50, true);
                data_to_leg(leg_joints, joints);
                leg_pub.publish(leg_joints);
            }
            position << 400, 280*(time_count-0.5)/0.5, 200;
            if(leftArmInverse(position, orientation, left_joints, false) == true)
            {
                left_arm_data.command[0] = left_joints(0);
                left_arm_data.command[1] = left_joints(1);
                left_arm_data.command[2] = left_joints(2);
                left_arm_data.command[3] = left_joints(3);
                left_arm_data.command[4] = left_joints(4);
                left_arm_data.command[5] = left_joints(5);
                left_arm_data.command[6] = left_joints(6);
                left_arm_pub.publish(left_arm_data);
            }

            position << -400, 280*(time_count-0.5)/0.5, 200;
            if(rightArmInverse(position, orientation, right_joints, false) == true)
            {
                right_arm_data.command[0] = right_joints(0);
                right_arm_data.command[1] = right_joints(1);
                right_arm_data.command[2] = right_joints(2);
                right_arm_data.command[3] = right_joints(3);
                right_arm_data.command[4] = right_joints(4);
                right_arm_data.command[5] = right_joints(5);
                right_arm_data.command[6] = right_joints(6);
                right_arm_pub.publish(right_arm_data);
            }
        }
        else if(time_count < 1.2)
        {
            orientation << 0,0,M_PI/4;
            position << 400-100*(time_count-1)/0.2, 280, 200;
            if(leftArmInverse(position, orientation, left_joints, true) == true)
            {
                left_arm_data.command[0] = left_joints(0);
                left_arm_data.command[1] = left_joints(1);
                left_arm_data.command[2] = left_joints(2);
                left_arm_data.command[3] = left_joints(3);
                left_arm_data.command[4] = left_joints(4);
                left_arm_data.command[5] = left_joints(5);
                left_arm_data.command[6] = left_joints(6);
                left_arm_pub.publish(left_arm_data);
            }

            orientation << 0,0,-M_PI/4;
            position << -400+125*(time_count-1)/0.2, 280, 200;
            if(rightArmInverse(position, orientation, right_joints, true) == true)
            {
                right_arm_data.command[0] = right_joints(0);
                right_arm_data.command[1] = right_joints(1);
                right_arm_data.command[2] = right_joints(2);
                right_arm_data.command[3] = right_joints(3);
                right_arm_data.command[4] = right_joints(4);
                right_arm_data.command[5] = right_joints(5);
                right_arm_data.command[6] = right_joints(6);
                right_arm_pub.publish(right_arm_data);
            }
        }
        else if(time_count < 1.25)
        {
            float angle = 1.5;
            right_finger_data.command[0] = angle;
            right_finger_data.command[1] = angle;
            right_finger_data.command[2] = angle;
            right_finger_data.command[3] = angle;
            right_finger_data.command[4] = angle;
            right_finger_data.command[5] = angle;
            right_finger_data.command[6] = angle;
            right_finger_data.command[7] = angle;
            right_finger_data.command[8] = angle;
            right_finger_data.command[9] = angle;
            right_finger_pub.publish(right_finger_data);

            left_finger_data.command[0] = angle;
            left_finger_data.command[1] = angle;
            left_finger_data.command[2] = angle;
            left_finger_data.command[3] = angle;
            left_finger_data.command[4] = angle;
            left_finger_data.command[5] = angle;
            left_finger_data.command[6] = angle;
            left_finger_data.command[7] = angle;
            left_finger_data.command[8] = angle;
            left_finger_data.command[9] = angle;
            left_finger_pub.publish(left_finger_data);
        }
        else if(leg_walk->Back(joints) == false)
        {
            data_to_leg(leg_joints, joints);
            leg_pub.publish(leg_joints);
        }
        else
        {
            if(leg_walk->get_status() == "standing")
            {
                leg_walk->Init(joints, 50, true);
                data_to_leg(leg_joints, joints);
                leg_pub.publish(leg_joints);
            }
            else
            {
                leg_walk->back(200, joints);
                data_to_leg(leg_joints, joints);
                leg_pub.publish(leg_joints);
            }
        }
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}